% implementation of 5.2 Tracking Control of a Single Euler–Lagrange System
% with parameters in P131 Theta2
% NT4GC5NN H. Cai, Y. Su, and J. Huang, Cooperative control of multi-agent systems: distributed-observer and distributed-internal-model approaches. in Advances in Industrial Control. Cham: Springer International Publishing, 2022. doi: 10.1007/978-3-030-98377-2.
% xsro@foxmail.com
params=struct();
params.Lambda=0.2*eye(5);
params.K=2000*eye(2);
params.alpha=10;
params.Theta=[0.76, 1.17, 0.14, 0.93, 0.44]';
initial=[zeros(4,1);(1:5)'];
[dxdt0,s0]=rhs(0,initial,params);

tfinal=300;
opt=odeset("OutputFcn","odeplot","OutputSel",5:9);
[t,x]=ode15s(@(t,x)rhs(t,x,params),[0,tfinal],initial,opt);
tic 
s=repmat(s0,length(t),1);
for i=1:length(t)
    [dxdti,si]=rhs(t(i),x(i,:)',params);
    s(i)=si;
end
toc
%%
figure(1);clf;
% 跟踪效果
nexttile;
q0=horzcat(s.q0)';
plot(t,x(:,1:2)-q0);
xlim([0,2])
grid on
legend(["$q-q_0$","$\dot{q}-\dot{q}_0$"],"Interpreter","latex")
title("track error")
% 参数估计收敛情况1
nexttile
hold on;
p=cell(length(params.Theta),1);
for i=1:length(params.Theta)
    p{i}=plot(t,x(:,5+i-1));
    plot([min(t) max(t)],[params.Theta(i) params.Theta(i)],"--","LineWidth",2,"Color",p{i}.Color)
end
ylim([0,6])
grid on
title("$\hat{\Theta}$ and $\Theta$","Interpreter","latex")
% 参数收敛情况
nexttile 
de=arrayfun(@(i)(s(i).Y*(x(i,5:9)'-params.Theta)),1:length(t),"UniformOutput",false);
semilogy(t,abs(cell2mat(de)))
title("$z=Y_i(\hat{\Theta}-\Theta)$","Interpreter","latex")
legend(["|z_1|","|z_2|"],"Interpreter","tex")
grid on
